
#include "autoSkeletonBuilder.h"

AutoSkelBuilder::AutoSkelBuilder() {
   mesh = 0;
   nIsoRings = 0;
   
   sumVerticesBand = Vector3(0,0,0);
   numVerticesBand = 0;
   currentBand = -1;
   bandbound = 0.1;
   activebandbound = bandbound;
   bandhasanewvertex = false;
}

AutoSkelBuilder::AutoSkelBuilder(Mesh *m) {
   
   assert(m != 0);
   mesh = m;
   nIsoRings = 5;
   
   sumVerticesBand = Vector3(0,0,0);
   numVerticesBand = 0;
   currentBand = -1;
   bandbound = 0.1;
   activebandbound = bandbound;
   bandhasanewvertex = false;
}

AutoSkelBuilder::~AutoSkelBuilder() {
   
}

GSkeleton* AutoSkelBuilder::create(const vector<Vertex_handle> &seeds) {
   
   assert(nIsoRings!=0);
   
   //priority.resize(seeds.size());
   //ordered_sources.resize(seeds.size());
   upper_bound.resize(seeds.size(), 1e6);
   
   cout<<"setMinDistance() ..."<<endl;
   setMinDistance(seeds);
   cout<<"setMinDistance() ... end"<<endl;
   
   cout<<"genIsoRings() ..."<<endl;
   genIsoRings();
   cout<<"genIsoRings() ... end"<<endl;
   
   return new GSkeleton(joints, graph);
}

GSkeleton* AutoSkelBuilder::create_usingwavefronts(const vector<Vertex_handle> &seeds) {
   
   cout<<""<<endl;
   if (seeds.empty()) return 0;
   
   openlists.clear();
   wavefronts.clear();
   
   for (uint i=0; i<seeds.size(); ++i) {
      Vertex_handle seed = seeds[i];
      
      WaveFront wf(i);
      OpenList o(seed, i, mesh, wf);
      
      openlists.push_back(o);
      wavefronts.push_back(wf);
   }
   
   computeUsingWaveFronts();
   
   return new GSkeleton(joints, graph);
}

void AutoSkelBuilder::allocatingWArrays() {
   
   assert(nseeds>0);
   
   distancesList.clear();
   for (uint i=0; i<mesh->size_of_vertices(); i++) {
      vector<Number> dlist(nseeds,0.0); /// memory allocation for geodesic distances
      distancesList.push_back(dlist);  /// geodesic metric
   }
}

/// preenche a idsource-th columna da tabela de distancias
void AutoSkelBuilder::setDistances(int idsource) {
   
   Vertex_iterator vit = mesh->vertices_begin();
   for (unsigned iv=0; iv<mesh->size_of_vertices(); iv++) {
      distancesList[iv][idsource] = vit[iv].d;
      vit[iv].d = std::numeric_limits<Number>::infinity();
   }
}

void AutoSkelBuilder::setMinDistance(const vector<Vertex_handle> &seeds) {
   
   if (seeds.empty()) return;
   
   //~ sortSeedsbyPriority(seeds);
   
   Vertex_iterator vi = mesh->vertices_begin();
   while (vi != mesh->vertices_end()) {
      vi->reset_clustering();
      ++vi;
   }
   
   // Fill the distance table (vertice x seeds)
   nseeds = seeds.size();
   int idsource = 0;
   
   allocatingWArrays();
   vector<Vertex_handle>::const_iterator it = seeds.begin();
   while (it != seeds.end()) {
      Vertex_handle hsource = *it++;
      mesh->floodfill_Dijkstra(hsource->id());
      setDistances(idsource);
      idsource++;
   }
   
   //assert(distancesList[0][0].size()>0);
   
   // Set {d,source} info in each vertex
   float MaxDistance = -1e10;
   Vertex_iterator vit = mesh->vertices_begin();
   for (unsigned iv=0; iv<mesh->size_of_vertices(); iv++) {
      int minsource = 0;
      Number dmin = distancesList[iv][0];
      for (int i=1; i<nseeds; ++i) {
         if (dmin > distancesList[iv][i]) {
            dmin = distancesList[iv][i];
            minsource = i;
         }
      }
      vit[iv].d = dmin;
      vit[iv].source(minsource);  // seed with d minimum
      
      assert(vit[iv].source() < (int)seeds.size());
      if (dmin > MaxDistance) MaxDistance = dmin;
   }
   intervalEquival = MaxDistance/(float)nIsoRings;
   cout<<"MaxDistance: "<<MaxDistance<<endl;
   cout<<"intervalEquival: "<<intervalEquival<<endl;
   
   /// compute the upper bounds
   cout<<"computeUpperBounds() begin"<<endl;
   computeUpperBounds();
   cout<<"computeUpperBounds() end"<<endl;
   
   /// computando os intervalos!
   vit = mesh->vertices_begin();
   for (unsigned iv=0; iv<mesh->size_of_vertices(); iv++) {
      vit[iv].inf((int)(vit[iv].d/intervalEquival));
      vit[iv].sup(vit[iv].inf());
   }
   
   for (unsigned iv=0; iv<mesh->size_of_vertices(); iv++) {
      Vertex_handle v = vit[iv].halfedge()->vertex();
      Halfedge_handle hh = v->halfedge(), hhbegin = hh;
      do {
         Vertex_handle vo = hh->opposite()->vertex();
         
         if(v->sup() < vo->inf())
            v->sup(vo->inf());
         
         if (v->dist_sup() < vo->d)
            v->dist_sup(vo->d);
         
         hh = hh->next()->opposite(); // 
      } while(hh != hhbegin);
   }
}

void AutoSkelBuilder::genIsoRings() {
   
   bonesl.clear();
   IsoRings.clear();
   parentNode.clear();
   joints.clear();
   
   Vertex_iterator vi = mesh->vertices_begin();
   for (unsigned i=0; i<mesh->size_of_vertices(); i++) {
      Vertex_handle v = vi[i].halfedge()->vertex();
      if (not v->is_classified()) {
         
         float custov = (v->inf()+0.5)*intervalEquival;
         float limv = min((v->inf()+1)*intervalEquival, upper_bound[v->source()]);
         stack<Vertex_handle> queue_vertices;
         queue_vertices.push(v);
         Ring pRing;
         Info info;
         info.plc.resize(nseeds,-1e8);
         info.pc.resize(nseeds);
         
         int idSourceMin = v->source();
         int iisolevel = v->inf();
         v->lastEC(joints.size());
         v->classified(true);
         pRing.push_back(v);
         
         int ncentros=1;
         Point3 centroide(v->point());
         v->idcentroid(joints.size());
         
         while (not queue_vertices.empty()) {
            Vertex_handle u = queue_vertices.top();
            queue_vertices.pop();
            
            if (u->inf() == iisolevel) {
               idSourceMin = u->source();
            }
            else {
               idSourceMin = -1;
            }
            
            Halfedge_handle hh = u->halfedge(), hhbegin = hh;
            do {
               Vertex_handle vo = hh->opposite()->vertex();
               int cent_size = joints.size();
               if ((iisolevel >= vo->inf()) and (iisolevel <= vo->sup())) {
                  
                  if (vo->inf() < iisolevel) {
                     if (vo->source() == idSourceMin) {
                        if (info.plc[idSourceMin] < vo->inf()) {
                           info.plc[idSourceMin] = vo->inf();
                           info.pc[idSourceMin] = vo->id();
                        }
                     }
                  }
                     
                  if (vo->lastEC() < cent_size) {
                     if (vo->inf() == iisolevel) {
                        vo->classified(true);
                        vo->idcentroid(joints.size());
                        pRing.push_back(vo);
                     }
                     
                     vo->lastEC(cent_size);
                     Point3 p = vo->point();
                     queue_vertices.push(vo);
                     
                     centroide = centroide + Vector3(p[0], p[1], p[2]);
                     ++ncentros;
                  }
               }
               //~ else {
                  //~ if (vo->lastEC() < cent_size) {
                     //~ if (vo->dist_sup() > upper_bound[vo->source()]) {
                        //~ if (vo->dist_sup() < limv) {
                           //~ queue_vertices.push(vo);
                           //~ vo->lastEC(cent_size);
                        //~ }
                     //~ }
                  //~ }
               //~ }
               hh = hh->next()->opposite(); // 
            } while(hh != hhbegin);
         }
         
         IsoRings.insert(make_pair(joints.size(), pRing));
         parentNode.push_back(info);
         
         centroide = Point3(centroide.x()/(float)ncentros, centroide.y()/(float)ncentros, centroide.z()/(float)ncentros);
         
         joints.push_back(centroide);
         costsl.push_back(custov);
      }
   }
   
   {
      Vertex_iterator vit = mesh->vertices_begin();
      for (unsigned iv=0; iv<mesh->size_of_vertices(); ++iv) { 
         assert(vit[iv].idcentroid() != -1);
      }
   }
   
   cout<<"xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx: "<<endl;
   cout<<"IsoRings.size(): "<<IsoRings.size()<<endl;
   { // set the ring's id for each vertex 
      multimap<int,Ring>::iterator it = IsoRings.begin();
      while (it != IsoRings.end()) {
         int i = it->first;
         cout<<"id: "<<i<<" :numberOfVertices: "<<it->second.size()<<endl;
         Ring &Ring = it->second;
         Ring::iterator itc = Ring.begin();
         while(itc != Ring.end()) {
            Vertex_handle vh = *itc++;
            vh->ringId(i);
         }
         ++it;
      }
   }
   
   graph.resize(joints.size());
   
   uint id = 0;
   Vertex_iterator vit = mesh->vertices_begin();
   vector<Info>::iterator itpN = parentNode.begin();
   while (itpN != parentNode.end()) {
      Info &pN = *itpN++;
      for (int i=0; i<nseeds; ++i) {
         if (pN.plc[i] != - 1e8) {
            pN.pc[i] = vit[pN.pc[i]].idcentroid();
            // cuidando para nao inserir duplicados
            if (find(graph[id].begin(),graph[id].end(), pN.pc[i]) == graph[id].end()) {
               graph[id].push_back(pN.pc[i]);
               graph[pN.pc[i]].push_back(id);
            }
         }
      }
      ++id;
   }
   
   //~ cout<<"graph graph graph: "<<endl;
   //~ for (uint i=0; i<graph.size(); ++i) {
      //~ copy(graph[i].begin(), graph[i].end(), ostream_iterator<Number>(cout, " "));
      //~ cout<<endl;
   //~ }
   //~ cout<<"xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx: "<<graph.size()<<endl;
   
   /// Build the bones list. Their vertices will associated in another function
   for (uint f=0; f<graph.size(); ++f) {
      const vector<int> &target = graph[f];
      for (uint c=0; c<target.size(); ++c) {
         uint cs = (f<target[c])? f : target[c];
         uint ct = (f<target[c])? target[c] : f;
         bone b(cs,ct); ring anel;
         bonesl.insert(make_pair(b, anel)); 
      }
   }
   
   /// Build the table joinseedsl
   joinseedsl.resize(joints.size());
   
   vector<vector<bool> > band_seeds;
   band_seeds.resize(joints.size());
   for (uint i=0; i<band_seeds.size(); ++i) {
      vector<bool> &f = band_seeds[i];
      f.resize(nseeds, false);
   }
   
   vit = mesh->vertices_begin();
   for (uint i=0; i<mesh->nvertices(); ++i) {
      band_seeds[vit[i].idcentroid()][vit[i].source()] = true;
   }
   
   for (uint i=0; i<band_seeds.size(); ++i) {
      vector<bool> &f = band_seeds[i];
      for (int j=0; j<f.size(); ++j) {
         if (band_seeds[i][j])
            joinseedsl[i].push_back(j);
      }
   }
   
   
   //~ joinseedsl.push_back(s);
   
   //~ cout<<"graph:! "<<endl;
   //~ vector<vector<int> >::iterator ita = graph.begin();
   //~ for (int f=0; f<graph.size(); ++f) {
      //~ float *pos = &joints[f*3];
      //~ vector<int> &target = graph[f];
      //~ cout<<f<<" <:> "<<"("<<pos[0]<<" "<<pos[1]<<" "<<pos[2]<<") <::> ";
      //~ for (int c=0; c<target.size(); ++c) {
         //~ cout<<target[c]<<" : ";
      //~ }
      //~ cout<<endl;
   //~ }
   
   //~ genRegularRings();
   
   // formando os novos rings (baseados na proejcao sobre a aresta do esqueleto)
   
   
   //cout<<"yyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyy"<<endl;
}

void AutoSkelBuilder::computeUpperBounds() {
   /*
   for (uint i=0; i<nseeds; ++i) {
      upper_bound[i] = 1e8;
      Vertex_iterator vi = mesh->vertices_begin();
      for (unsigned ii=0; ii<mesh->size_of_vertices(); ++ii) {
         if (vi->source() == ordered_sources[i]) {
            Halfedge_handle hh = vi->halfedge(), hhbegin = hh;
            do {
               Vertex_handle w = hh->opposite()->vertex();
               if (priority[w->source()] > priority[ordered_sources[i]]) {
                  float *pv = &mesh->vpositions[vi->id()];
                  float *pw = &mesh->vpositions[w->id()];
                  const Vector3 vw(pv[0]-pw[0],pv[1]-pw[1],pv[2]-pw[2]);
                  const Number dvw = sqrt(vw*vw);
                  float dist_temp = vi->d() + dvw;
                  if (dist_temp < upper_bound[i])
                     upper_bound[i] = dist_temp;
               }
               hh = hh->next()->opposite(); // 
            } while(hh != hhbegin);
         }
         ++vi;
      }
      
      vi = model->vertices_begin();
      for (unsigned ii=0; ii<model->size_of_vertices(); ++ii) {
         if (vi->source() == ordered_sources[i]) {
            if (vi->d() > upper_bound[i]) {
               vi->d(1e8);
               for (int j=i+1; j<nseeds; ++j) {
                  float &d = (*distancesList[vi->id()/3])[ordered_sources[j]];
                  if (d < vi->d()) {
                     vi->d(d);
                     vi->source(ordered_sources[j]);
                  }
               }
            }
         }
         ++vi;
      }
   }
   
   //~ cout<<"upper_bound upper_bound upper_bound upper_bound upper_bound"<<endl;
   copy(upper_bound.begin(), upper_bound.end(), ostream_iterator<float>(cout, " "));
   */
}

Vector3 unitary(Halfedge_handle h) {
   
   const Point3 &p = h->vertex()->point();
   const Point3 &q = h->opposite()->vertex()->point();
   Vector3 v = p-q;
   return v/sqrt(v*v);
}

Number lenght(Halfedge_handle h) {
   
   const Point3 &p = h->vertex()->point();
   const Point3 &q = h->opposite()->vertex()->point();
   Vector3 v = p-q;
   return sqrt(v*v);
}

float compute_penalty(float cost, WaveFront &wf, Vertex_handle m, 
                      const Vector3 &uhp, const Vector3 &uha, const Vector3 &uhn) {
   
   float penalty;
   float weightcost=1, weightedges=1.0, weightnormals=0; float lowerbound = 0.05;
   
   penalty = weightcost*cost;
   //~ float norma = sqrt(wf.dpEdges*wf.dpEdges);
                                   
   float norma = sqrt(uhp*uhp);
                                   
   const Point3 &p = m->point();
   Vector3 v(p[0],p[1],p[2]);
   Vector3 dif = v - wf.masscenter;
                                   
   
   if (norma > lowerbound) {
      //~ penalty += (dif*wf.dpEdges/norma)*weightedges;
      penalty += (dif*uhp/norma)*weightedges;
   }
   
   return penalty;
   
   norma = sqrt(wf.dpNormals*wf.dpNormals);
   if (norma > lowerbound) {
      float norma2=dif*dif;
      if (norma2 < 10e-8) return penalty;
      
      dif = dif/sqrt(dif*dif);
      Vector3 vecprod = cross_product(uhp,uha+uhn);
      if (vecprod*vecprod < 10e-8) return penalty;
      
      vecprod = vecprod/sqrt(vecprod*vecprod);
      float cos = vecprod*dif;
      float cos2 = cos*cos;
      if (cos2 < 10e-8) return penalty;
      penalty += weightnormals*sqrt(norma2*(1-cos2)/cos2);
   }
   
   return penalty;
}

void AutoSkelBuilder::computeUsingWaveFronts() {
   
   std::vector<Point3> centroides;
   
   if (wavefronts.empty() or openlists.empty()) return;
      
   //~ wavefronts[0].print();
   //~ openlists[0].print(model);
   //~ cout<<"hello graph::computeUsingWaveFronts pasando la criacao de openlists e wavefronts"<<endl;
   
   bool thereisactivewf;
   do {
      float minray2 = 10e8;
      bool stillactive; 
      thereisactivewf=false;
      int ind_min_ray2;
      for (uint i=0; i<wavefronts.size(); ++i) {
         //~ cout<<"i: "<<i<<" wavefronts[i].activity: "<<wavefronts[i].activity<<endl;
         if (not wavefronts[i].activity) continue;
         
         stillactive = false;
         int currNode = wavefronts[i].firstNode;
         
         //~ cout<<"% % % currNode: "<<currNo   de<<endl;
         int ii=0;
         do {
            ++ii;
            //~ if (ii>500) exit(0);
            //~ cout<<"%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%: "<<i<<endl;
            OpenList &o = openlists[wavefronts[i].idseed];
            //~ if (not o.nodes[currNode].active_indicator) {
            if (o.nodes[currNode].h->idclosestSeed() != wavefronts[i].idseed) {
               currNode = o.nodes[currNode].nodeant;
            }
            else {
               //~ cout<<" else =========================== "<<endl;
               stillactive = true;
               thereisactivewf = true;
               if (wavefronts[i].ray2 < minray2) {
                  minray2 = wavefronts[i].ray2;
                  ind_min_ray2 = i;
               }
               break;
            }
            
         } while (currNode != wavefronts[i].firstNode);
         
         if (not stillactive) {
            int tmpcurrnode = wavefronts[i].firstNode;
            OpenList &o = openlists[wavefronts[i].idseed];
            do {
               //~ cout<<"o.nodes[tmpcurrnode].h->idclosestSeed(): "<<o.nodes[tmpcurrnode].h->idclosestSeed()<<endl;
               //~ cout<<"wavefronts[i].idseed: "<<wavefronts[i].idseed<<endl;
               tmpcurrnode = o.nodes[tmpcurrnode].nodeant;
            } while (tmpcurrnode != wavefronts[i].firstNode);
            wavefronts[i].activity = false;
         }
      }
      
      //~ cout<<"X X X X h e l l o o              o o o o o  o  o !: "<<ind_min_ray2<<" :: "<<wavefronts.size()-1<<endl;
      // selecao da wavefront que vai ser expandida
      if (thereisactivewf) {
         
         int oldestnode = 1e8;
         
         //~ sumVerticesBand = wavefronts[ind_min_ray2].sumVertices;
         //~ numVerticesBand = wavefronts[ind_min_ray2].nNodes;
         
         sumVerticesBand = Vector3(0,0,0);
         numVerticesBand = 0;
         int curr_node = wavefronts[ind_min_ray2].firstNode;
         do {
            OpenList &o = openlists[wavefronts[ind_min_ray2].idseed];
            o.nodes[curr_node].h->last_seed_reaching_vertex(wavefronts[ind_min_ray2].idseed);
            
            NodeStack ns(curr_node, o.nodes[curr_node].penalty);
            o.queue.push(ns);
            
            if (curr_node < oldestnode) {
               wavefronts[ind_min_ray2].oldestnode = curr_node;
               oldestnode = curr_node;
            }
            
            //~ float *p = &model->vpositions[o.nodes[curr_node].h->id()];
            //~ sumVerticesBand = sumVerticesBand + Vector3(p[0],p[1],p[2]);
            //~ ++numVerticesBand ;
            curr_node = o.nodes[curr_node].nodeant;
         } while (curr_node != wavefronts[ind_min_ray2].firstNode);
         
         ++currentBand;
         
         parentband.push_back(-1);
         //~ if (wavefronts[ind_min_ray2].idseed != ind_min_ray2) {
            //~ cout<<"currentBand: "<<currentBand<<" <> "<<parentband.size()<<" ^^^ "<<wavefronts[ind_min_ray2].band<<endl;
            parentband[currentBand] = wavefronts[ind_min_ray2].band;
         //~ }
         
         int lastwavefront = wavefronts.size()-1;
         ExpandingTree(ind_min_ray2, lastwavefront, centroides);
         
         
         
         //~ cout<<"xxx ::::::::: wavefronts.size(): "<<wavefronts.size()<<endl;
         //~ for (int i=0; i<wavefronts.size(); ++i) {
            //~ wavefronts[i].print();
         //~ }
         
         //~ exit(0);
      }
   }
   while (thereisactivewf);
   //~ cout<<"::::::::: wavefronts.size(): "<<wavefronts.size()<<endl;
   //~ for (int i=0; i<wavefronts.size(); ++i) {
      //~ wavefronts[i].print();
   //~ }
   
   
   /*
   cout<<"jj jj jj jj jj jj wavefronts "<<wavefronts.size()<<endl;
   for (int i=0; i<wavefronts.size(); ++i) {
      if (wavefronts[i].nNodes > 1) {
         int kk=0;
         int curr_node = wavefronts[i].firstNode;
            do {
            OpenList &o = openlists[0];
            float *p = &model->vpositions[o.nodes[curr_node].h->id()];
            cout<<"vertex: "<<o.nodes[curr_node].h->id()/3<<" "<<p[0]<<" "<<p[1]<<" "<<p[2]<<endl;
            o.nodes[curr_node].print();
            curr_node = o.nodes[curr_node].nodeant;
            ++kk;
         } while (curr_node != wavefronts[i].firstNode and kk < 500);
      }
   }
   
   for (int i=0; i<wavefronts.size(); ++i) 
      wavefronts[i].print();
   cout<<"o.print()"<<endl;
   openlists[0].print(model);
   */
   
   cout<<"!!!centroides.size()!!!: "<<centroides.size()<<endl;
   // // // copying the joinssssssssssss begin!
   joints = centroides;
//   d.resize(joints.size());
   
   cout<<"graph::computeUsingWaveFronts ... done!"<<endl;
}

void AutoSkelBuilder::ExpandingTree(int idwf, int lastwavefront, vector<Point3> &centroides) {
   
   //~ cout<<"+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + "<<endl;
   //~ cout<<"+ + + + + + + + + + + + + + + graph::ExpandingTree() begin"<<endl;
   OpenList &o = openlists[wavefronts[idwf].idseed];
   
   
   activebandbound = bandbound;
   bandhasanewvertex = false;
   bool validnodeflag = true;
   
   //~ cout<<"oooooooooooooooooooooooooooooooooooooooooooo "<<endl;
   do {
      //~ cout<<"+ + + pilha pilha: "<<endl;
      NodeStack ns;
      do {
         if (o.queue.empty()) { 
            validnodeflag = false; 
            break;
         }
         
         //~ o.print_queue();
         
         ns = o.queue.top();
         o.queue.pop();
         
         //float *p = &model->vpositions[o.nodes[ns.nodeid].h->id()];
         //~ cout<<"    Node: "<<o.nodes[ns.nodeid].h->id()/3<<" ~ "<<ns.nodeid<<" <> "<<ns.penalty<<" ~ "<<Point3(p[0],p[1],p[2])<<endl;
         //~ cout<<"   ??????: "<<o.nodes[ns.nodeid].wfront<<" ~~ "<<idwf<<" >< "<<o.queue.size()<<endl;
         
      } while ((o.nodes[ns.nodeid].penalty != ns.penalty) or
               (not o.nodes[ns.nodeid].active_indicator) );
      
      //~ ++ii;
      //~ if (ii>100) exit(0);
      
      //~ cout<<"hola 2"<<endl;
      if (not validnodeflag) {  break;}
      else if (o.nodes[ns.nodeid].cost > activebandbound) {
         
         //~ cout<<"else iffffffffffffff x"<<endl;
         
         if (bandhasanewvertex) {
         
            //~ cout<<"]]]]]]]]]]]]] "<<sumVerticesBand<<" <> "<<numVerticesBand<<endl;
            
            wavefronts[idwf].masscenter = wavefronts[idwf].sumVertices/(float)wavefronts[idwf].nNodes;
               
            //~ Vector3 v = sumVerticesBand/(float)numVerticesBand;
            //~ Point3 c(v[0], v[1], v[2]);
            //~ centroides.push_back(c);
            
            //~ cout<<"ns.nodeid: "<<ns.nodeid<<" <><><><><><><><><> "<<c<<" "<<o.nodes[ns.nodeid].cost<<" ~ "<<o.nodes[ns.nodeid].penalty<<" "<<activebandbound<<endl;
            
            //~ sumVerticesBand = Vector3(0,0,0);
            //~ numVerticesBand = 0;
            
            //~ cout<<"wwwwwwwwwwwwwwwwww qqqqqqqqqqqqqqqq"<<endl;
            
            vector<int> tmp;
            tmp.push_back(idwf);
            for (int i=1; i<wavefronts.size()-lastwavefront; ++i) {
               tmp.push_back(wavefronts.size()-i);
            }
            
            for (int i=0; i<tmp.size(); ++i) {
               
               WaveFront &wfront = wavefronts[tmp[i]];
               if (not wfront.activity) continue;
               int tmpindex = wfront.firstNode;
               wfront.band = currentBand;
               //~ Node &tmp = o.nodes[tmpindex];
                  
               do {
                  
                  //~ cout<<"WWWW WWWWWWWWWWWWWWWWWW tmpindex: "<<tmpindex<<endl;
                  
                  const Point3 &p = o.nodes[tmpindex].h->point();
                  const Vector3 &q = wfront.masscenter;
                  Vector3 ray(p[0]-q[0],p[1]-q[1],p[2]-q[2]);
                  if (ray*ray > wfront.ray2)
                     wfront.ray2 = ray*ray;
                     
                  const Vector3 uhp = unitary(o.nodes[tmpindex].hparent);
                  const Vector3 uha = unitary(o.nodes[tmpindex].hprev);
                  const Vector3 uhn = unitary(o.nodes[tmpindex].hnext);
                  o.nodes[tmpindex].penalty = compute_penalty(0, wfront, o.nodes[tmpindex].h, uhp, uha, uhn);
                  o.nodes[tmpindex].cost = 0;
                  
                  
                  tmpindex = o.nodes[tmpindex].nodeant;
                  
               } while (tmpindex != wfront.firstNode);
            }
            break;
         }
         else {
            //~ cout<<"PPPPPPPPPPPPPPPPPPPPPPPPPPPP"<<endl;
            activebandbound = ((int)(o.nodes[ns.nodeid].cost/bandbound)+1)*bandbound;
            //~ cout<<"xxxxx AAAAAAAAAAAAAAAAAAA xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx"<<(int)(o.nodes[ns.nodeid].cost/bandbound)<<endl;
            //~ cout<<"aaa "<<activebandbound<<endl;
            //~ cout<<"aaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa hola closing_nodes: (ns.nodeid): "<<ns.nodeid<<" "<<idwf<<endl;
            //~ wf.print();
            int nodeidwf = o.nodes[ns.nodeid].wfront;
            //~ wavefronts[nodeidwf].print();
            //~ cout<<"nnnnnnnnnnnnnnnnnnnnodeidwf: "<<nodeidwf<<endl;
            closing_nodes(ns.nodeid, nodeidwf);
         }
      }
      else {
         //~ cout<<"aaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa hola closing_nodes: (ns.nodeid): "<<ns.nodeid<<" "<<idwf<<endl;
         //~ wf.print();
         int nodeidwf = o.nodes[ns.nodeid].wfront;
         //~ wavefronts[nodeidwf].print();
         //~ cout<<"nnnnnnnnnnnnnnnnnnnnodeidwf: "<<nodeidwf<<endl;
         closing_nodes(ns.nodeid, nodeidwf);
      }
      //~ cout<<"hola 3"<<endl;
      
   } while (true);
   
   if (numVerticesBand>0) {
      Vector3 v = sumVerticesBand/(float)numVerticesBand;
      Point3 c(v[0], v[1], v[2]);
      centroides.push_back(c);
      //~ parentband.push_back(0);
   }
   else {
      Point3 p = centroides.back();
      centroides.push_back(p);
   }
   
   while (not o.queue.empty()) o.queue.pop();
      
   
   
   //~ cout<<"finalllllllllllllllllllllllllllllll"<<endl;
   //~ o.print(model);
   
   // // // copying the joinssssssssssss end!
   
   //~ cout<<"graph::ExpandingTree() end: "<<centroides.size()<<endl;
   
}

void AutoSkelBuilder::closing_nodes(int nodeid, int idwf) {
   
   //~ cout<<"<<< @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ >>>> closing_nodes() begin ..."<<nodeid<<" "<<idwf<<" :::: "<<endl;
   //~ wf.print();
   //~ cout<<"xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx"<<endl;
   
   OpenList &o = openlists[wavefronts[idwf].idseed];
   if (o.nodes[nodeid].nodeant == o.nodes[nodeid].nodepost) {
      o.nodes[o.nodes[nodeid].nodepost].penalty = -1;
      o.nodes[nodeid].penalty = -1;
      o.nodes[o.nodes[nodeid].nodepost].active_indicator = 0;
      o.nodes[nodeid].active_indicator= 0;
      
      //~ cout<<"XX X  X XXXXXXXXXXXXXXXXX xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx: "<<nodeid<<endl;
      
      wavefronts[idwf].activity = false;
      wavefronts[idwf].nNodes -= 2;
      return;
   }
   
   Vertex_handle n = o.nodes[nodeid].h;
   Halfedge_handle heparent = o.nodes[nodeid].hprev->next();
   Vertex_handle m = heparent->vertex();
   Halfedge_handle heant = heparent->next()->opposite();
   
   const Point3 &p = m->point();
   const Point3 &q = n->point();
   Vector3 parentedge(p[0]-q[0],p[1]-q[1],p[2]-q[2]);
   int nodeant = o.nodes[nodeid].nodeant;
   Vertex_handle nant = o.nodes[nodeant].h;
   
   //~ cout<<"111111111111111111111111111111111111111111111"<<endl;
   bool topology_change_tag = false;
   //~ while (m != o.nodes[nodeid].hnext->vertex()) {
   int tmpnodepost = o.nodes[nodeid].nodepost;
   
   int jj=0;
   
   //~ cout<<"222222222222222222222222222222222222"<<endl;
   while (m->id() != o.nodes[tmpnodepost].h->id()) {
      
      ++jj;
      
               
      //~ cout<<m->last_seed_reaching_vertex()<<" "<<wavefronts[idwf].idseed<<endl;
      if (m->last_seed_reaching_vertex() == wavefronts[idwf].idseed) { 
         //~ cout<<"iffffffffffffffff"<<endl;
         //~ exit(0);
         if (heant == o.nodes[nodeant].hprev->opposite()) { // repeticao de aresta
            
            //~ cout<<" # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # "<<endl;
            //~ cout<<nodeid<<" "<<nodeant<<" "<<o.nodes[nodeid].nodepost<<endl;
            //~ --wf.nNodes;
            --wavefronts[idwf].nNodes;
            
            const Point3 &q = nant->point();
            const Vector3 &n = nant->normal();
            wavefronts[idwf].sumVertices = wavefronts[idwf].sumVertices - Vector3(q[0],q[1],q[2]);
            wavefronts[idwf].sumEdges = wavefronts[idwf].sumEdges - unitary(o.nodes[nodeant].hparent);
            wavefronts[idwf].sumNormals = wavefronts[idwf].sumNormals - Vector3(n[0],n[1],n[2]);
            
            wavefronts[idwf].dpEdges = wavefronts[idwf].sumEdges/(float)wavefronts[idwf].nNodes;
            wavefronts[idwf].dpNormals = wavefronts[idwf].sumNormals/(float)wavefronts[idwf].nNodes;
            wavefronts[idwf].masscenter = wavefronts[idwf].sumVertices/(float)wavefronts[idwf].nNodes;
            
            int newnodeant = o.nodes[nodeant].nodeant;
            o.nodes[nodeid].hprev = heparent->opposite();
            o.nodes[newnodeant].hnext = heparent->opposite();
            o.nodes[nodeid].nodeant = newnodeant;
            o.nodes[newnodeant].nodepost = nodeid;
            o.nodes[nodeant].active_indicator = false;
            o.nodes[nodeant].penalty = -1;
            
            wavefronts[idwf].firstNode = newnodeant;
            nodeant = newnodeant;
         }
         else { // repeticao de node
            
            //~ cout<<" % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % %  "<<endl;
            
            topology_change_tag = true;
            
            WaveFront wavefront;
            wavefront.idseed = wavefronts[idwf].idseed;
            wavefront.nNodes = 1;
            
            const Point3 &p = m->point();
            const Vector3 &n = m->normal();
            wavefront.sumVertices = Vector3(p[0],p[1],p[2]);
            wavefront.sumEdges = Vector3(0,0,0);
            wavefront.sumNormals = Vector3(n[0],n[1],n[2]);
            
            wavefront.dpEdges = Vector3(0,0,0);
            wavefront.dpNormals = Vector3(0,0,0);
            wavefront.masscenter = Vector3(0,0,0);
            
            wavefront.ray2 = 0;
            wavefront.activity = true;
            wavefront.idseed = wavefronts[idwf].idseed;
            wavefront.firstNode = nodeant;
            
            //~ cout<<"**************************** : "<<nodeant<<" "<<o.nodes[nodeid].h->id()/3<<endl;
            //~ o.print(model);
            
            
            int ii=0;
            int curr_node = nodeant;
            do {
               
               //~ cout<<"rrrrrrrrrrrrrrrrrrrrrrrrrrrrrr"<<endl;
               /*
               if (ii>500) { 
                  
                  for (int i=0; i<wavefronts.size(); ++i) 
                     wavefronts[i].print();
                  cout<<"o.print()"<<endl;
                  o.print(model);
                  
                  cout<<curr_node<<" "<<o.nodes[curr_node].h->id()/3<<" ~~ "<<m->id()/3<<endl;
                  exit(0);
               }
               */
               
               o.nodes[curr_node].wfront = wavefronts.size();
               ++wavefront.nNodes;
               const Point3 &p = o.nodes[curr_node].h->point();
               const Vector3 &n = o.nodes[curr_node].h->normal();
               wavefront.sumVertices = wavefront.sumVertices + Vector3(p[0],p[1],p[2]);
               wavefront.sumEdges = wavefront.sumEdges + unitary(o.nodes[curr_node].hparent);
               wavefront.sumNormals = wavefront.sumNormals + Vector3(n[0],n[1],n[2]);
               
               curr_node = o.nodes[curr_node].nodeant;
               
               ++ii;
            } while ((o.nodes[curr_node].h->id() != m->id()) and (curr_node != nodeant));
            
            if (curr_node != nodeant) {
               //~ cout<<"**************************** >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> "<<endl;
               
               /// ??????????????
               wavefront.sumEdges = wavefront.sumEdges + unitary(o.nodes[curr_node].hparent);
               
               
               wavefront.dpEdges = wavefront.sumEdges/(float)wavefront.nNodes;
               wavefront.dpNormals = wavefront.sumNormals/(float)wavefront.nNodes;
               wavefront.masscenter = wavefront.sumVertices/(float)wavefront.nNodes;
               
               //~ cout<<":: "<<wavefronts[idwf].nNodes<<" :::: "<<wavefront.nNodes<<endl;
               wavefronts[idwf].nNodes -= (wavefront.nNodes - 1);
               // inserindo nova wf
               wavefronts.push_back(wavefront);
               
               /// Um node copia???
               Node newnode;
               newnode.h = m;
               newnode.hprev = heant;
               newnode.hnext = o.nodes[curr_node].hnext;
               newnode.hparent = o.nodes[curr_node].hparent;
               
               newnode.nodeant = nodeant;
               newnode.nodepost = o.nodes[curr_node].nodepost;
               newnode.wfront = wavefronts.size()-1;
               newnode.idseed = wavefronts[idwf].idseed;
               
               float tmpcost = o.nodes[nodeid].cost + lenght(heparent);
               if (tmpcost < o.nodes[curr_node].cost) {
                  newnode.cost = tmpcost;
                  o.nodes[curr_node].cost = tmpcost;
               }
               else {
                  newnode.cost =  o.nodes[curr_node].cost;
               }
               
               Vector3 uhp = unitary(heparent);
               Vector3 uha = unitary(heant);
               Vector3 uhn = unitary(heparent->opposite()->prev());
               float tmppenalty= compute_penalty(newnode.cost, wavefronts[idwf], m, uhp, uha, uhn);
               
               if (tmppenalty < o.nodes[curr_node].penalty) {
                  newnode.penalty = tmppenalty;
                  o.nodes[curr_node].penalty = tmppenalty;
                  
                  NodeStack ns(curr_node, tmppenalty);
                  o.queue.push(ns);
               }
               else {
                  newnode.penalty =  o.nodes[curr_node].penalty;
               }
               
               newnode.active_indicator = o.nodes[curr_node].active_indicator;
               
               //~ cout<<"%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%"<<endl;
               //~ o.print_queue();
               
               o.pushback(newnode);
               //~ cout<<"%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -- "<<endl;
               //~ o.print_queue();
               //~ cout<<"%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%"<<endl;
               
               o.nodes[nodeant].nodepost = o.nodes.size() -1;
               o.nodes[nodeant].hnext = heant;
               int newnodepost = o.nodes[curr_node].nodepost;
               o.nodes[newnodepost].nodeant = o.nodes.size() -1;
               
               o.nodes[curr_node].hnext = heparent->opposite();
               o.nodes[nodeid].hprev = heparent->opposite();
               
               nodeant = curr_node;
               o.nodes[nodeid].nodeant = nodeant;
               o.nodes[nodeant].nodepost = nodeid;
            }
         }
         //~ cout<<"wwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwww"<<endl;
         //~ o.print(model);
      }
      else { // criacao de um node 
         //~ cout<<"elseeeeeeeeeeeeeeeeeeeeeeeee criacao de um node !!!!!!!!!!!!!!!!!!!!!!!!!!!!"<<endl;
         //~ o.print(model);
         
         //~ exit(0);
         
         m->last_seed_reaching_vertex(wavefronts[idwf].idseed);
         
         Node newnode;
         newnode.h = m;
         newnode.hprev = heant;
         newnode.hnext = heparent->opposite();
         //~ newnode.hnext = heparent->opposite()->prev();
         newnode.hparent = heparent;
         
         Vector3 uhp = unitary(heparent);
         Vector3 uha = unitary(heant);
         Vector3 uhn = unitary(heparent->opposite()->prev());
         
         newnode.nodepost = nodeid;
         newnode.nodeant = nodeant;
         newnode.wfront = idwf;
         newnode.idseed = wavefronts[idwf].idseed;
         newnode.cost = o.nodes[nodeid].cost + lenght(heparent);
         newnode.penalty = compute_penalty(newnode.cost, wavefronts[idwf], m, uhp, uha, uhn);
         
         if (m->idclosestSeed() == -1) {
            newnode.active_indicator = true;
            m->idclosestSeed(wavefronts[idwf].idseed);
         }
         else if (m->idclosestSeed() != wavefronts[idwf].idseed)
            newnode.active_indicator = false;
            
         //~ newnode.print();
         o.pushback(newnode);
         
         //~ NodeStack ns(o.nodes.size(), newnode.penalty);
         //~ cout<<"aa"<<endl;
         //~ o.nodes.push_back(newnode);
         //~ cout<<"bb"<<endl;
         //~ o.queue.push(ns);
         
         //~ cout<<"22222222222222222222"<<endl;
         
         int newnodeid = o.nodes.size()-1;
         o.nodes[nodeant].nodepost = newnodeid;
         o.nodes[nodeant].hnext = heant;
         o.nodes[nodeid].hprev = heparent->opposite();
         o.nodes[nodeid].nodeant = newnodeid;
         
         //~ float *p = &model->vpositions[m->id()];
         //~ sumVerticesBand = sumVerticesBand + Vector3(p[0],p[1],p[2]);
         //~ ++numVerticesBand;
         //~ m->band(currentBand);
         //~ bandhasanewvertex = true;
         
         
         ++wavefronts[idwf].nNodes;
         const Point3 &q = m->point();
         const Vector3 &n = m->normal();
         wavefronts[idwf].sumVertices = wavefronts[idwf].sumVertices + Vector3(q[0],q[1],q[2]);
         wavefronts[idwf].sumEdges = wavefronts[idwf].sumEdges + unitary(heparent);
         wavefronts[idwf].sumNormals = wavefronts[idwf].sumNormals + Vector3(n[0],n[1],n[2]);
      }
      
      heparent = o.nodes[nodeid].hprev->next();
      m = heparent->vertex();
      heant = heparent->next()->opposite();
      
      nodeant = o.nodes[nodeid].nodeant;
      nant = o.nodes[nodeant].h;
   }
   
   //~ cout<<"closing_nodes() ... aaaaaaaaaaaaaaaaaaaaaaaaaaa"<<endl;
   
   int nodepostid = o.nodes[nodeid].nodepost;
   if (o.nodes[nodepostid].nodepost == nodeant) {
      o.nodes[nodepostid].penalty = -1;
      o.nodes[nodeant].penalty = -1;
      o.nodes[nodepostid].active_indicator = 0;
      o.nodes[nodeant].active_indicator= 0;
      
      //~ cout<<"u u u u u u uuuuuuuuuuuuuuuuuuuuuuuuuuuuuuuuuuuuuuuuuuuu"<<endl;
      //~ cout<<nodeid<<" "<<nodeant<<" "<<nodepostid<<" idwf: "<<idwf<<endl;
      
      wavefronts[idwf].activity = false;
      wavefronts[idwf].nNodes -= 2;
   }
   else {
      o.nodes[nodeant].nodepost = o.nodes[nodeid].nodepost;
      o.nodes[nodepostid].nodeant = nodeant;
      //~ o.nodes[nodepostid].hprev = o.nodes[nodeid].hnext->next()->opposite();
      o.nodes[nodepostid].hprev = heant;
      o.nodes[nodeant].hnext = o.nodes[nodepostid].hprev;
      wavefronts[idwf].firstNode = nodeant;
   
      if (topology_change_tag) {
         
         int curr_node = nodeant;
         wavefronts[idwf].nNodes = 1;
         
         const Point3 &p = o.nodes[curr_node].h->point();
         const Vector3 &n = o.nodes[curr_node].h->normal();
         wavefronts[idwf].sumVertices = Vector3(p[0],p[1],p[2]);
         wavefronts[idwf].sumEdges = unitary(o.nodes[curr_node].hparent);
         wavefronts[idwf].sumNormals = Vector3(n[0],n[1],n[2]);
         
         int tmpnodepost = o.nodes[nodeid].nodepost;
         while (o.nodes[curr_node].h != o.nodes[tmpnodepost].h) {
            
            //~ cout<<"+++++++++++++++++++++++++++++++++++++++++"<<endl;
            curr_node = o.nodes[curr_node].nodeant;
            
            ++wavefronts[idwf].nNodes;
            
            const Point3 &p = o.nodes[curr_node].h->point();
            const Vector3 &n = o.nodes[curr_node].h->normal();
            wavefronts[idwf].sumVertices = wavefronts[idwf].sumVertices + Vector3(p[0],p[1],p[2]);
            wavefronts[idwf].sumEdges = wavefronts[idwf].sumEdges + unitary(o.nodes[curr_node].hparent);
            wavefronts[idwf].sumNormals = wavefronts[idwf].sumNormals + Vector3(n[0],n[1],n[2]);
         }
      }
      else {
         --wavefronts[idwf].nNodes;
         const Point3 &p = o.nodes[nodeid].h->point();
         const Vector3 &n = o.nodes[nodeid].h->normal();
         wavefronts[idwf].sumVertices = wavefronts[idwf].sumVertices - Vector3(p[0],p[1],p[2]);
         wavefronts[idwf].sumEdges = wavefronts[idwf].sumEdges - unitary(o.nodes[nodeid].hparent);
         wavefronts[idwf].sumNormals = wavefronts[idwf].sumNormals - Vector3(n[0],n[1],n[2]);
      }
      wavefronts[idwf].dpEdges = wavefronts[idwf].sumEdges/(float)wavefronts[idwf].nNodes;
      wavefronts[idwf].dpNormals = wavefronts[idwf].sumNormals/(float)wavefronts[idwf].nNodes;
      wavefronts[idwf].masscenter = wavefronts[idwf].sumVertices/(float)wavefronts[idwf].nNodes;
   }
   
   
   int tempidwf = o.nodes[nodeid].wfront;
   if (wavefronts[tempidwf].nNodes > 5) {
      Vector3 tempcg = wavefronts[tempidwf].sumVertices/(float)wavefronts[tempidwf].nNodes;
      //~ cout<<"wavefronts[tempidwf].nNodes: "<<wavefronts[tempidwf].nNodes<<endl;
      
      const Point3 &p = n->point();
      //~ sumVerticesBand = sumVerticesBand + Vector3(pp[0],pp[1],pp[2]);
      sumVerticesBand = sumVerticesBand + tempcg;
      ++numVerticesBand;
   }
   
   
   if (o.nodes[nodeid].active_indicator)
      n->band(currentBand);
   bandhasanewvertex = true;
   
   o.nodes[nodeid].penalty = -1;
   o.nodes[nodeid].active_indicator = false;
   
   //~ cout<<"final u u u u u u uuuuuuuuuuuuuuuuuuuuuuuuuuuuuuuuuuuuuuuuuuuu"<<endl;
   //~ cout<<nodeid<<" "<<nodeant<<" "<<nodepostid<<" "<<endl;
   
   //~ cout<<"&&&&&&&&&&&&&&&&&&&&&& closing_nodes() end ..."<<endl;
   //~ o.print(model);
}
